#pragma once

#include "rclcpp/rclcpp.hpp"
#include "tf2/LinearMath/Quaternion.hpp"
#include "tf2_ros/transform_broadcaster.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include "geometry_msgs/msg/transform_stamped.hpp"
#include <geometry_msgs/msg/twist.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <sensor_msgs/msg/imu.hpp>
#include <std_msgs/msg/string.hpp>
#include <std_msgs/msg/float32.hpp>

#include <chrono>
#include <memory>
#include "hardware_controller/serial_parser.h"

using namespace std;

namespace base_controller {

class BaseController : public rclcpp::Node {
    public:
        BaseController();
        ~BaseController();
    
        void OdomReset();
        void ControlCore();
        void Publish_Odom();        //发布里程计话题
        void Publish_Voltage();     //发布电源电压话题
        void Publish_Imu();         //发布IMU话题

        void cmdvel_cb(const geometry_msgs::msg::Twist::SharedPtr vel_msg);

    private:
        // ros::NodeHandle nh_;
        // ros::Publisher odom_pub_,imu_pub_,voltage_pub_;
        // ros::Subscriber cmdvel_sub_;
        
        rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_pub_;
        rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imu_pub_;
        rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr voltage_pub_;
        rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmdvel_sub_;
        std::unique_ptr<tf2_ros::TransformBroadcaster> br_;
    
        // void cmdvel_cb(const geometry_msgs::Twist::ConstPtr& vel_msg);
    
        string serial_port_,base_frame_,odom_frame_,imu_frame_;
        int serial_baud_;
        double robot_x_,robot_y_,robot_yaw_;
        
        // ros::Time time_now_,time_last_,speed_time_last_;
        rclcpp::Time time_now_,time_last_,speed_time_last_;
        float Sampling_Time; 
    
        SerialParser* serial_parser_;  
        bool odom_tf_;
    };

};  //base_controller